/*
 * Copyright (c) 2021, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package boofcv.alg.distort.kanbra;

/**
 * Common functions for computing the forward and inverse model.
 *
 * @author Peter Abeles
 */
public class KannalaBrandtUtils_F64 {
	public static double polynomial( double[] coefs, double x ) {
		double pow = x;
		double result = 0;
		for (int i = 0; i < coefs.length; i++) {
			result += coefs[i]*pow;
			pow *= x*x;
		}
		return result;
	}

	public static double polytrig( double[] coefs, double cos, double sin ) {
		if (coefs.length == 0)
			return 0.0;

		double result = 0;
		result += coefs[0]*cos;
		result += coefs[1]*sin;

		// sin(2*phi) = 2*cos(phi)*sin(phi)
		// cos(2*phi) = 2*cos^2(phi)-1
		result += coefs[2]*2.0*cos*sin;
		result += coefs[3]*(2.0*cos*cos - 1.0);

		return result;
	}

	/**
	 * Computes the gradient as a function of the coefficients
	 */
	public static void polytrigGradient( double cos, double sin, double[] gradient ) {
		gradient[0] = cos;
		gradient[1] = sin;
		gradient[2] = 2.0*cos*sin;
		gradient[3] = 2.0*cos*cos - 1.0;
	}

	public static double polynomialDerivative( double[] coefs, double x ) {
		double pow = 1.0;
		double left = 1.0;
		double result = 0.0;
		for (int i = 0; i < coefs.length; i++) {
			result += left*coefs[i]*pow;
			pow *= x*x;
			left += 2;
		}
		return result;
	}

	public static double polytrigDerivative( double[] coefs, double cos, double sin ) {
		if (coefs.length == 0)
			return 0.0;

		double result = 0;
		result -= coefs[0]*sin;
		result += coefs[1]*cos;
		result += 2*coefs[2]*(cos*cos - sin*sin);
		result -= 4*coefs[3]*cos*sin;

		return result;
	}
}
